﻿#include "mpu6050.h"

#include <stdio.h>

#include "elog.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "main.h"
#include "math.h"
#include "stm32f1xx_hal.h"
#define LOG_TAG "mpu6050"
/* The sensors can be mounted onto the board in any orientation. The mounting
 * matrix seen below tells the MPL how to rotate the raw data from thei
 * driver(s).
 * TODO: The following matrices refer to the configuration on an internal test
 * board at Invensense. If needed, please modify the matrices to match the
 * chip-to-body matrix for your particular set up.
 */
static signed char gyro_orientation[9] = { -1, 0, 0,
                                           0, -1, 0,
                                           0, 0, 1 };
static mpu6050_Obj mpu6050;
/* These next two functions converts the orientation matrix (see
 * gyro_orientation) to a scalar representation for use by the DMP.
 * NOTE: These functions are borrowed from Invensense's MPL.
 */
static unsigned short inv_row_2_scale( const signed char* row )
{
    unsigned short b;

    if ( row[0] > 0 )
        b = 0;
    else if ( row[0] < 0 )
        b = 4;
    else if ( row[1] > 0 )
        b = 1;
    else if ( row[1] < 0 )
        b = 5;
    else if ( row[2] > 0 )
        b = 2;
    else if ( row[2] < 0 )
        b = 6;
    else
        b = 7; // error
    return b;
}

static unsigned short inv_orientation_matrix_to_scalar(
    const signed char* mtx )
{
    unsigned short scalar;

    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale( mtx );
    scalar |= inv_row_2_scale( mtx + 3 ) << 3;
    scalar |= inv_row_2_scale( mtx + 6 ) << 6;

    return scalar;
}

static int run_self_test( void )
{
    int result;
    long gyro[3], accel[3];

    result = mpu_run_self_test( gyro, accel );
    if ( result == 0x3 )
    {
        /* Test passed. We can trust the gyro data here, so let's push it down
         * to the DMP.
         */
        float sens;
        unsigned short accel_sens;
        mpu_get_gyro_sens( &sens );
        gyro[0] = (long)( gyro[0] * sens );
        gyro[1] = (long)( gyro[1] * sens );
        gyro[2] = (long)( gyro[2] * sens );
        dmp_set_gyro_bias( gyro );
        mpu_get_accel_sens( &accel_sens );
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias( accel );
    }
    else
    {
        return -1;
    }

    return 0;
}

static int MPU6050_DMP_Init_Internal( void )
{
    int ret;
    struct int_param_s int_param;
    //mpu_init
    ret = mpu_init( &int_param );
    if ( ret != 0 )
    {
        return ERROR_MPU_INIT;
    }
    //设置传感器
    ret = mpu_set_sensors( INV_XYZ_GYRO | INV_XYZ_ACCEL );
    if ( ret != 0 )
    {
        return ERROR_SET_SENSOR;
    }
    //设置fifo
    ret = mpu_configure_fifo( INV_XYZ_GYRO | INV_XYZ_ACCEL );
    if ( ret != 0 )
    {
        return ERROR_CONFIG_FIFO;
    }
    //设置采样率
    ret = mpu_set_sample_rate( DEFAULT_MPU_HZ );
    if ( ret != 0 )
    {
        return ERROR_SET_RATE;
    }
    //加载DMP固件
    ret = dmp_load_motion_driver_firmware();
    if ( ret != 0 )
    {
        return ERROR_LOAD_MOTION_DRIVER;
    }
    //设置陀螺仪方向
    ret = dmp_set_orientation( inv_orientation_matrix_to_scalar( gyro_orientation ) );
    if ( ret != 0 )
    {
        return ERROR_SET_ORIENTATION;
    }
    //设置DMP功能
    ret = dmp_enable_feature(
        DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL );
    if ( ret != 0 )
    {
        return ERROR_ENABLE_FEATURE;
    }
    //设置输出速率
    ret = dmp_set_fifo_rate( DEFAULT_MPU_HZ );
    if ( ret != 0 )
    {
        return ERROR_SET_FIFO_RATE;
    }
    //自检
    ret = run_self_test();
    if ( ret != 0 )
    {
        return ERROR_SELF_TEST;
    }
    //使能DMP
    ret = mpu_set_dmp_state( 1 );
    if ( ret != 0 )
    {
        return ERROR_DMP_STATE;
    }

    return 0;
}

int MPU6050_DMP_Init( mpu6050_Obj* mpu6050data )
{
    // 如果已经初始化，则直接返回成功
    if ( mpu6050.initialized )
    {
        return 0; // 已经初始化
    }
    HAL_NVIC_DisableIRQ( EXTI9_5_IRQn );
    int ret;
    // 不断尝试初始化，直到成功
    do
    {
        ret = MPU6050_DMP_Init_Internal();
        if ( ret == 0 )
        {
            mpu6050.initialized = 1; // 设置初始化成功标志
            log_i( "MPU6050 succeed" );
        }
        else
        {
            log_d( "MPU6050 failing，ret: %d", ret );
            //LED_RED
            HAL_GPIO_TogglePin( RED_LED_GPIO_Port, RED_LED_Pin );
            HAL_Delay( 1000 );
        }
    } while ( ret != 0 );
    HAL_GPIO_WritePin( RED_LED_GPIO_Port, RED_LED_Pin, GPIO_PIN_SET );
    HAL_NVIC_EnableIRQ( EXTI9_5_IRQn );
    return ret;
}

float pitch, roll, yaw, accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;

static int MPU6050_Get_Data( void )
{
    float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
    short gyro[3];
    short accel[3];
    long quat[4];
    unsigned long timestamp;
    short sensors;
    unsigned char more;

    if ( dmp_read_fifo( gyro, accel, quat, &timestamp, &sensors, &more ) )
    {
        return -1;
    }

    // 获取四元数
    if ( sensors & INV_WXYZ_QUAT )
    {
        q0 = quat[0] / Q30;
        q1 = quat[1] / Q30;
        q2 = quat[2] / Q30;
        q3 = quat[3] / Q30;

        // 计算姿态角
        mpu6050.pitch = asin( -2 * q1 * q3 + 2 * q0 * q2 ) * 57.3;                                      // pitch
        mpu6050.roll = atan2( 2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1 ) * 57.3;       // roll
        mpu6050.yaw = atan2( 2 * ( q0 * q3 + q1 * q2 ), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3 ) * 57.3; // yaw
    }

    // 将加速度和角速度转换为浮点值并传回
    mpu6050.accel_x = accel[0] / 16384.0; // 传感器灵敏度是 16384 LSB/g
    mpu6050.accel_y = accel[1] / 16384.0;
    mpu6050.accel_z = accel[2] / 16384.0;

    mpu6050.gyro_x = gyro[0] / 131.0; // 传感器灵敏度是 131 LSB/°/s
    mpu6050.gyro_y = gyro[1] / 131.0;
    mpu6050.gyro_z = gyro[2] / 131.0;

    return 0;
}

void MPU6050_IRQHandler( void )
{
    if ( mpu6050.data_flag == 1 )
    {
        MPU6050_Get_Data();
    }
}

mpu6050_Obj* create_MPU6050_Obj( void )
{
    if ( !mpu6050.initialized )
    {
        MPU6050_DMP_Init( &mpu6050 );     // 初始化单例
        mpu6050.run = MPU6050_IRQHandler; // 设置函数指针
    }
    return &mpu6050;
}